<!-- saved from url=(0014)about:internet -->
<pre class="code">
<span class="srcline"><span class="lineno"><a href="1,1" id="srcline1"> 1</a></span><span class="line"><span class="keyword">function</span> [ <span class="var type1" id="S2T9U3">h</span> <span class="var type1" id="S3T9U4">v</span> <span class="var type1" id="S4T9U5">a</span> ] = nav_h_ekf(<span class="var type1" id="S5T2U8">Q</span>, <span class="var type1" id="S6T12U9">H</span>, <span class="var type1" id="S7T14U10">R</span> ,<span class="var type1" id="S8T2U11">F</span>, <span class="var type1" id="S9T2U12">I</span> ,<span class="var type1" id="S10T18U13">Z</span> )</span></span>
<span class="srcline"><span class="lineno"><a href="1,2" id="srcline2"> 2</a></span><span class="line"><span class="comment">%作者 ytf</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,3" id="srcline3"> 3</a></span><span class="line"><span class="comment">%日期 2017年4月27日</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,4" id="srcline4"> 4</a></span><span class="line"><span class="comment">%状态转移协方差矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,5" id="srcline5"> 5</a></span><span class="line"><span class="comment">%H 预测矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,6" id="srcline6"> 6</a></span><span class="line"><span class="comment">%R 观测噪声方差</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,7" id="srcline7"> 7</a></span><span class="line"><span class="comment">%F 状态转移矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,8" id="srcline8"> 8</a></span><span class="line"><span class="comment">%I</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,9" id="srcline9"> 9</a></span><span class="line"><span class="comment">%Z 测量矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,10" id="srcline10">10</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="1,11" id="srcline11">11</a></span><span class="line"><span class="keyword">persistent</span> <span class="var type1" id="S11T3U15">X</span> ;<span class="comment">% 状态变量</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,12" id="srcline12">12</a></span><span class="line">          <span class="keyword">if</span>(<span class="mxinfo" id="T1:U11"><span class="mxinfo" id="T1:U12">isempty(<span class="var type0" id="S11T0U21">X</span>)</span></span>)</span></span>
<span class="srcline"><span class="lineno"><a href="1,13" id="srcline13">13</a></span><span class="line">           <span class="mxinfo" id="T3:U13"><span class="var type1" id="S11T3U24">X</span>=<span class="mxinfo" id="T3:U15">single(<span class="mxinfo" id="T19:U16">[0 0 0 0]'</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,14" id="srcline14">14</a></span><span class="line">          <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,15" id="srcline15">15</a></span><span class="line">          </span></span>
<span class="srcline"><span class="lineno"><a href="1,16" id="srcline16">16</a></span><span class="line"> <span class="keyword">persistent</span> <span class="var type1" id="S14T2U35">P</span> ;<span class="comment">% 状态协方差矩阵</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,17" id="srcline17">17</a></span><span class="line">            <span class="keyword">if</span>(<span class="mxinfo" id="T1:U18"><span class="mxinfo" id="T1:U19">isempty(<span class="var type0" id="S14T0U41">P</span>)</span></span>)</span></span>
<span class="srcline"><span class="lineno"><a href="1,18" id="srcline18">18</a></span><span class="line">            <span class="mxinfo" id="T2:U20"><span class="var type1" id="S14T2U44">P</span>=<span class="mxinfo" id="T2:U22">single(<span class="mxinfo" id="T20:U23">[10 0 0 0;</span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,19" id="srcline19">19</a></span><span class="line"><span class="mxinfo" id="T2:U20"><span class="mxinfo" id="T2:U22"><span class="mxinfo" id="T20:U23">                     0 10 0 0;</span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,20" id="srcline20">20</a></span><span class="line"><span class="mxinfo" id="T2:U20"><span class="mxinfo" id="T2:U22"><span class="mxinfo" id="T20:U23">                     0 0 10 0;</span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,21" id="srcline21">21</a></span><span class="line"><span class="mxinfo" id="T2:U20"><span class="mxinfo" id="T2:U22"><span class="mxinfo" id="T20:U23">                     0 0 0  100]</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,22" id="srcline22">22</a></span><span class="line">            <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,23" id="srcline23">23</a></span><span class="line">       <span class="mxinfo" id="T3:U24"><span class="var type1" id="S11T3U70">X</span>=<span class="mxinfo" id="T3:U26"><span class="var type1" id="S8T2U72">F</span>*<span class="var type1" id="S11T3U73">X</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,24" id="srcline24">24</a></span><span class="line">       <span class="mxinfo" id="T2:U29"><span class="var type1" id="S14T2U76">P</span>=<span class="mxinfo" id="T2:U31"><span class="mxinfo" id="T2:U32"><span class="mxinfo" id="T2:U33"><span class="var type1" id="S8T2U80">F</span>*<span class="var type1" id="S14T2U81">P</span></span>*<span class="mxinfo" id="T2:U36"><span class="var type1" id="S8T2U83">F</span>'</span></span>+<span class="var type1" id="S5T2U84">Q</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,25" id="srcline25">25</a></span><span class="line">       <span class="mxinfo" id="T13:U39"><span class="var type1" id="S15T13U87">K</span>=<span class="mxinfo" id="T13:U41"><span class="mxinfo" id="T13:U42"><span class="var type1" id="S14T2U90">P</span>*<span class="mxinfo" id="T13:U44"><span class="var type1" id="S6T12U92">H</span>'</span></span>/(<span class="mxinfo" id="T14:U46"><span class="mxinfo" id="T14:U47"><span class="mxinfo" id="T12:U48"><span class="var type1" id="S6T12U97">H</span>*<span class="var type1" id="S14T2U98">P</span></span>*<span class="mxinfo" id="T13:U51"><span class="var type1" id="S6T12U100">H</span>'</span></span> +<span class="var type1" id="S7T14U101">R</span></span>)</span></span>; <span class="comment">%增益</span></span></span>
<span class="srcline"><span class="lineno"><a href="1,26" id="srcline26">26</a></span><span class="line">       <span class="mxinfo" id="T3:U54"><span class="var type1" id="S11T3U104">X</span>=<span class="mxinfo" id="T3:U56"><span class="var type1" id="S11T3U106">X</span>+<span class="mxinfo" id="T3:U58"><span class="var type1" id="S15T13U108">K</span>*(<span class="mxinfo" id="T18:U60"><span class="var type1" id="S10T18U111">Z</span>-<span class="mxinfo" id="T18:U62"><span class="var type1" id="S6T12U113">H</span>*<span class="var type1" id="S11T3U114">X</span></span></span>)</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,27" id="srcline27">27</a></span><span class="line">       <span class="mxinfo" id="T2:U65"><span class="var type1" id="S14T2U117">P</span>=<span class="mxinfo" id="T2:U67">(<span class="mxinfo" id="T2:U68">single(<span class="var type1" id="S9T2U123">I</span>)-<span class="mxinfo" id="T2:U70"><span class="var type1" id="S15T13U125">K</span>*<span class="var type1" id="S6T12U126">H</span></span></span>)*<span class="var type1" id="S14T2U127">P</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,28" id="srcline28">28</a></span><span class="line">      </span></span>
<span class="srcline"><span class="lineno"><a href="1,29" id="srcline29">29</a></span><span class="line">       <span class="mxinfo" id="T9:U74"><span class="var type1" id="S2T9U130">h</span>=<span class="mxinfo" id="T9:U76"><span class="var type1" id="S11T3U132">X</span>(<span class="mxinfo" id="T6:U78">1</span>)</span></span></span></span>
<span class="srcline"><span class="lineno"><a href="1,30" id="srcline30">30</a></span><span class="line">       <span class="mxinfo" id="T9:U79"><span class="var type1" id="S3T9U136">v</span>=<span class="mxinfo" id="T9:U81"><span class="var type1" id="S11T3U138">X</span>(<span class="mxinfo" id="T6:U83">2</span>)</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="1,31" id="srcline31">31</a></span><span class="line">       <span class="mxinfo" id="T9:U84"><span class="var type1" id="S4T9U142">a</span>=<span class="mxinfo" id="T9:U86"><span class="var type1" id="S11T3U144">X</span>(<span class="mxinfo" id="T6:U88">3</span>)</span></span>;</span></span>
</pre>
